#include "lidar_cells.h"
namespace perception
{
    namespace algorithm
    {
        int LidarCells::Process(LidarPoint &data_in, Localization &loc,
                                std::vector<std::vector<data_cell>> &lidar_cells,
                                std::vector<LidarObject> &lidar_objs)
        {
            if (data_in.points->points.empty())
            {
                RCLCPP_WARN(private_nh_->get_logger(),
                            "lidar_cells no lidar points input!");
                return 1;
            }
            time = data_in.time;
            const auto a = data_in.points;
            clusters.clear();
            {
                auto cluster_ = new EuclideanCluster(false, 2, 2000, 1.5);
                pcl::PointCloud<pcl::PointXYZI>::Ptr raw_pointcloud_ptr(
                    new pcl::PointCloud<pcl::PointXYZI>);
                pcl::PointCloud<pcl::PointXYZI>::Ptr raw_pointcloud_ptr2(
                    new pcl::PointCloud<pcl::PointXYZI>);
                for (auto p : a->points)
                {
                    if (std::fabs(p.x > 30) && std::fabs(p.x < 50))
                    {
                        raw_pointcloud_ptr2->points.push_back(p);
                        raw_pointcloud_ptr->points.push_back(p);
                    }
                    else if (std::fabs(p.x >= 50))
                        raw_pointcloud_ptr2->points.push_back(p);
                    else
                        raw_pointcloud_ptr->points.push_back(p);
                }
                // pcl::copyPointCloud(*data_in.points, *raw_pointcloud_ptr);
                // raw_pointcloud_ptr->points = data_in.points;
                cluster_->cluster(raw_pointcloud_ptr2, clusters);
                cluster_->setTolerance(0.7);
                cluster_->setMinClusterSize(5);
                cluster_->cluster(raw_pointcloud_ptr, clusters);
            }
            // {
            //     RCLCPP_INFO(private_nh_->get_logger(), " 111111");
            //     pcl::PointCloud<pcl::PointXYZI>::Ptr raw_pointcloud_ptr(
            //         new pcl::PointCloud<pcl::PointXYZI>);
            //     pcl::copyPointCloud(*data_in.points, *raw_pointcloud_ptr);
            //     pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
            //     tree->setInputCloud(raw_pointcloud_ptr);
            //     std::vector<pcl::PointIndices> cluster_indices;
            //     DBSCANKdtreeCluster<pcl::PointXYZI> ec;
            //     ec.setCorePointMinPts(3);
            //     ec.setClusterTolerance(0.2);
            //     ec.setMinClusterSize(5);
            //     ec.setMaxClusterSize(1000);
            //     ec.setSearchMethod(tree);
            //     ec.setInputCloud(raw_pointcloud_ptr);
            //     ec.extract(cluster_indices);
            //     RCLCPP_INFO(private_nh_->get_logger(), " cluster_indices  size:%d", cluster_indices.size());
            //     for (const auto &cluster : cluster_indices)
            //     {
            //         pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZI>);
            //         for (const auto &point_idx : cluster.indices)
            //         {
            //             cloud_cluster->points.push_back(raw_pointcloud_ptr->points[point_idx]);
            //         }
            //         clusters.push_back(*cloud_cluster);
            //         clusters.back().width = cloud_cluster->points.size();
            //         clusters.back().height = 1;
            //         clusters.back().is_dense = false;
            //     }
            //     RCLCPP_INFO(private_nh_->get_logger(), " 222222");
            // }
            RCLCPP_INFO(private_nh_->get_logger(), " clusters  size:%d", clusters.size());
            this->GridCell(a);
            if (this->PushCell(loc, lidar_cells, lidar_objs) < 0)
                return -1;
            RCLCPP_INFO(private_nh_->get_logger(), " lidar_objs  size:%d", lidar_objs.size());
            // // 删除位置碰撞的目标
            if (lidar_objs.size() > 3)
            {
                for (int i = 0; i < lidar_objs.size() - 2; i++)
                {
                    for (int j = i + 1; j < lidar_objs.size() - 1; j++)
                    {
                        OBB::Box2d obb1{lidar_objs[i].x, lidar_objs[i].y, lidar_objs[i].heading, lidar_objs[i].length, lidar_objs[i].width}; // 位于原点，宽度为4，高度为2，无旋转
                        OBB::Box2d obb2{lidar_objs[j].x, lidar_objs[j].y, lidar_objs[j].heading, lidar_objs[j].length, lidar_objs[j].width}; // 位于(5,2)，宽度为3，高度为1，无旋转

                        auto is_colliding = OBB::HasOverlap(obb1, obb2);
                        if (is_colliding)
                        {
                            std::cout << " ############# \n";
                            std::cout << "i:" << i << " j:" << j << std::endl;
                            std::cout << "i x:"
                                      << "" << lidar_objs[i].x << " i y:" << lidar_objs[i].y << " id:" << lidar_objs[i].id << std::endl;
                            std::cout << "j x:"
                                      << "" << lidar_objs[j].x << " j y:" << lidar_objs[j].y << " id:" << lidar_objs[j].id << std::endl;
                            lidar_objs[i].cluster_points.points.insert(lidar_objs[i].cluster_points.points.end(),
                                                                       lidar_objs[j].cluster_points.points.begin(),
                                                                       lidar_objs[j].cluster_points.points.end());
                            classify_->Compute(lidar_objs[i]);
                            lidar_objs.erase(lidar_objs.begin() + j);
                            i = 0;
                            j = i + 1;
                            break;
                        }
                    }
                }
            }
            data_cell_tmp = data_cell_;
            lidar_cells = data_cell_;
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    data_cell_[i][j].reset();
                }
            }
            // delete cluster_;
            return 0;
        }

        int LidarCells::GridCell(const pcl::PointCloud<XYZI_1>::Ptr &data_in)
        {
            for (auto &p : data_in->points)
            {
                int x =
                    int(std::round((p.x - cell_config_.xmin) / cell_config_.cell_size_x));
                int y =
                    int(std::round((p.y - cell_config_.ymin) / cell_config_.cell_size_y));
                // 跳过检测范围外的
                if (x > row - 1 || y > col - 1 || x < 0 || y < 0)
                    continue;
                // 累计高度
                lidar_cells_grid_->Grid(x, y, p.z, p.x, p.y, data_cell_);
                lidar_cells_grid_->DistValid(x, y, data_cell_);
            }
            return 0;
        }

        int LidarCells::PushCell(Localization &loc,
                                 std::vector<std::vector<data_cell>> &lidar_cells,
                                 std::vector<LidarObject> &lidar_objs)
        {
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    auto time = std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::system_clock::now().time_since_epoch());
                    // 点云位置的时间补偿

                    // data_cell_[i][j].x = dvx * (1.0 / hz) + data_cell_[i][j].x;
                    // data_cell_[i][j].y = dvy * (1.0 / hz) + data_cell_[i][j].y;

                    lidar_cells_grid_->GetConfidence(i, j, data_cell_, data_cell_tmp);
                    lidar_cells_grid_->GetValid(i, j, 2, data_cell_);
                    data_cell_[i][j].time = time.count();
                }
            }
            RCLCPP_INFO(private_nh_->get_logger(), " clusters objs size:%d", clusters.size());
            std::vector<LidarObject> objs;
            if (clusters.empty())
                return -1;
            for (int i = 0; i < clusters.size(); i++)
            {
                LidarObject obj;
                if (clusters[i].points.empty())
                    continue;
                obj.cluster_points = clusters[i];
                objs.emplace_back(obj);
            }
            // classify

            classify_->Compute(objs);
            if (1)
            {
                // 合并比较近的目标
                for (int i = 0; i < objs.size() - 1; i++)
                {
                    // if (objs[i].x > 15 && objs[i].x < 25 && objs[i].y < 0 && objs[i].y > -10)
                    //     std::cout << "id:" << objs[i].id << " objs[i].x:" << objs[i].x << " objs[i].y:" << objs[i].y << std::endl;
                    for (int j = i + 1; j < objs.size(); j++)
                    {
                        if (std::fabs(objs[i].x - objs[j].x) + std::fabs(objs[i].y - objs[j].y) < 2.5)
                        {
                            std::cout << "i:" << i << " j:" << j << std::endl;
                            std::cout << "i x:"
                                      << "" << objs[i].x << " i y:" << objs[i].y << " id:" << objs[i].id << std::endl;
                            std::cout << "j x:"
                                      << "" << objs[j].x << " j y:" << objs[j].y << " id:" << objs[j].id << std::endl;
                            objs[i].cluster_points.points.insert(objs[i].cluster_points.points.end(),
                                                                 objs[j].cluster_points.points.begin(),
                                                                 objs[j].cluster_points.points.end());
                            classify_->Compute(objs[i]);
                            objs.erase(objs.begin() + j);
                            i = -1;
                            j = i + 1;
                            break;
                        }
                    }
                }
                // 合并碰撞目标
                if (objs.size() > 3)
                {
                    for (int i = 0; i < objs.size() - 2; i++)
                    {
                        for (int j = i + 1; j < objs.size() - 1; j++)
                        {
                            OBB::Box2d obb1{objs[i].x, objs[i].y, objs[i].heading, objs[i].length, objs[i].width}; // 位于原点，宽度为4，高度为2，无旋转
                            OBB::Box2d obb2{objs[j].x, objs[j].y, objs[j].heading, objs[j].length, objs[j].width}; // 位于(5,2)，宽度为3，高度为1，无旋转

                            auto is_colliding = OBB::HasOverlap(obb1, obb2);
                            if (is_colliding)
                            {
                                std::cout << " ~~~~~~~~~~~~~~~~~` \n";
                                std::cout << "i:" << i << " j:" << j << std::endl;
                                std::cout << "i x:"
                                          << "" << objs[i].x << " i y:" << objs[i].y << " id:" << objs[i].id << std::endl;
                                std::cout << "j x:"
                                          << "" << objs[j].x << " j y:" << objs[j].y << " id:" << objs[i].id << std::endl;
                                objs[i].cluster_points.points.insert(objs[i].cluster_points.points.end(),
                                                                     objs[j].cluster_points.points.begin(),
                                                                     objs[j].cluster_points.points.end());
                                classify_->Compute(objs[i]);
                                objs.erase(objs.begin() + j);
                                i = -1;
                                j = i + 1;
                                break;
                            }
                        }
                    }
                }
            }
            // debug
            for (auto obj : objs)
            {
                std::cout << "x:" << obj.x << " y:" << obj.y << " id:" << obj.id << std::endl;
            }
            RCLCPP_INFO(private_nh_->get_logger(), " classify objs size:%d", objs.size());
            // cell_config_.track_config_.time_eps) 暂时不做时间对齐,只要激光和loc都有数据
            if (loc.time > 1 && time > 1)
            {
                RCLCPP_INFO(private_nh_->get_logger(), "start track");
                // loc.time = time;
                track_->Process2(objs, loc);
                // RCLCPP_INFO(private_nh_->get_logger(), "1111");
                std::vector<LidarObject> objs_tmp;
                // for (auto &a : track_->now_objs)
                for (auto &a : track_->now_objs_center)
                {
                    // RCLCPP_INFO(private_nh_->get_logger(), " a.x:%f,a.y:%f,a.width_box:%f,a.length_box:%f,", a.x, a.y, a.width_box, a.length_box);
                    // 去除刚跟踪的目标，长宽阈值，
                    if (a.length_box > 10 || a.width_box > 10)
                        continue;
                    // if (std::fabs(a.x - a.cp.x) + std::fabs(a.y - a.cp.y) > 10)
                    //     continue;
                    // if (std::fabs(a.x - a.cp.x) + std::fabs(a.y - a.cp.y) < 0.1)
                    //     continue;
                    // 后面看可能把未匹配目标也先滤出
                    if (a.track_type_ == track_type::MATCHING_FAILED)
                        continue;
                    // track_type::NEW_OBJ)
                    //     continue;
                    if (a.height_box < 0.1 && a.z < 0.3)
                        continue;
                    if (isnan(a.x) || isnan(a.y) || isnan(a.cvx) || isnan(a.cvy) ||
                        isnan(a.length_box) || isnan(a.width_box))
                        continue;

                    LidarObject track_obj;
                    track_obj.x = a.x;
                    track_obj.y = a.y;
                    // track_obj.x = a.cx;
                    // track_obj.y = a.cy;
                    track_obj.z = a.z;
                    track_obj.vx = a.cvx;
                    track_obj.vy = a.cvy;
                    track_obj.length = a.length_box;
                    track_obj.width = a.width_box;
                    track_obj.height = a.height_box;
                    track_obj.id = a.id;
                    track_obj.heading = a.psi;
                    track_obj.age = a.age;
                    track_obj.type = a.type;
                    track_obj.corner_point = a.cp;
                    track_obj.confidence = a.confidence;
                    track_obj.corner_points = a.all_points;
                    objs_tmp.emplace_back(track_obj);
                }

                CoordinateTransformation(objs_tmp, loc);
                // 去除一些长宽比太大的误检
                // for (int i = objs_tmp.size() - 1; i >= 0; i--)
                // {
                //     if (objs_tmp[i].x < 50)
                //     {
                //         if (objs_tmp[i].height < 0.1)
                //             objs_tmp.erase(objs_tmp.begin() + i);
                //     }
                //     // if (objs_tmp[i].length / objs_tmp[i].width > 7)
                //     //     objs_tmp.erase(objs_tmp.begin() + i);
                // }

                lidar_objs = objs_tmp;
            }
            else
            {
                RCLCPP_INFO(private_nh_->get_logger(),
                            "loc and lidar timestamp is not matched,no track");

                lidar_objs = objs;
            }

            return 0;
        }
        void LidarCells::CoordinateTransformation(std::vector<LidarObject> &cells,
                                                  Localization &loc)
        {
            auto tmp = cells;
            for (size_t i = 0; i < cells.size(); i++)
            {
                cells[i].heading = loc.angle * deg2rad + cells[i].heading;
                cells[i].x = (tmp[i].x - loc.xg) * std::cos(loc.angle * deg2rad) +
                             (tmp[i].y - loc.yg) * std::sin(loc.angle * deg2rad);
                cells[i].y = (tmp[i].y - loc.yg) * std::cos(loc.angle * deg2rad) -
                             (tmp[i].x - loc.xg) * std::sin(loc.angle * deg2rad);
                cells[i].vx = (tmp[i].vx) * std::cos(loc.angle * deg2rad) +
                              (tmp[i].vy) * std::sin(loc.angle * deg2rad);
                cells[i].vy = (tmp[i].vy) * std::cos(loc.angle * deg2rad) -
                              (tmp[i].vx) * std::sin(loc.angle * deg2rad);
                cells[i].corner_point.x =
                    (tmp[i].corner_point.x - loc.xg) * std::cos(loc.angle * deg2rad) +
                    (tmp[i].corner_point.y - loc.yg) * std::sin(loc.angle * deg2rad);
                cells[i].corner_point.y =
                    (tmp[i].corner_point.y - loc.yg) * std::cos(loc.angle * deg2rad) -
                    (tmp[i].corner_point.x - loc.xg) * std::sin(loc.angle * deg2rad);
                for (auto &cell : cells[i].corner_points)
                {
                    auto tmp_cell = cell;
                    cell.x = (tmp_cell.x - loc.xg) * std::cos(loc.angle * deg2rad) +
                             (tmp_cell.y - loc.yg) * std::sin(loc.angle * deg2rad);
                    cell.y = (tmp_cell.y - loc.yg) * std::cos(loc.angle * deg2rad) -
                             (tmp_cell.x - loc.xg) * std::sin(loc.angle * deg2rad);
                }
            }
        }
    } // namespace algorithm
} // namespace perception